﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using System.Runtime.InteropServices;
#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/diff?spec=svn33&r=33&format=side&path=/trunk/Box2D/Box2D/Common/b2Math.h
// Revision   : r112
// Change-Date: 2010-06-17
//
// Source-File: ???
// Revision   : ???
// Change-Date: ???
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
        //
        // Original Port without special optimizations
        //

#if !OPTIMIZE
    public static partial class Math
    {
        public static readonly ushort USHRT_MAX = 0xffff;
        public static readonly byte UCHAR_MAX = 0xff;
        public static readonly int RAND_LIMIT = 32767;

        public static readonly double PI = System.Math.PI;
        public static readonly double TwoPI = PI * 2.0;

        private static Random s_rnd = new Random();

        /// <summary>
        /// This function is used to ensure that a floating point number is
        /// not a NaN or infinity.
        /// </summary>
        public static bool IsValid(float x)
        {
            if (float.IsNaN(x)) return false;
            return float.NegativeInfinity < x && x < float.PositiveInfinity;
        }

        [StructLayout(LayoutKind.Explicit)]
        public struct Convert 
        { 
            [FieldOffset(0)]
            public float x;

            [FieldOffset(0)] 
            public int i;
        }

        /// <summary> 
        /// This is a approximate yet fast inverse square-root. 
        /// </summary> 
        public static float InvSqrt(float x)
        {
            Convert convert = new Convert();
            convert.x = x;
            float xhalf = 0.5f * x;
            convert.i = 0x5f3759df - (convert.i >> 1);
            x = convert.x;
            x = x * (1.5f - xhalf * x * x);
            return x;
        }

        /// <summary>
        /// Random number in range [-1,1]
        /// </summary>
        public static float Random()
        {
            float r = (float)(s_rnd.Next() & RAND_LIMIT);
            r /= RAND_LIMIT;
            r = 2.0f * r - 1.0f;
            return r;
        }

        public static Mat22 Abs(Mat22 A)
        {
            return new Mat22(A.col1.Abs(), A.col2.Abs());
        }

        public static void Swap<T>(ref T a, ref T b)
        {
            T tmp = a;
            a = b;
            b = tmp;
        }

        /// <summary> 
        /// Multiply a matrix times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another. 
        /// </summary> 
        public static Vector2 Mul(Mat22 A, Vector2 v)
        {
            Vector2 u;
            u = new Vector2(A.col1.X * v.X + A.col2.X * v.Y, A.col1.Y * v.X + A.col2.Y * v.Y);
            return u;
        }

        /// <summary> 
        /// Multiply a matrix transpose times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another (inverse transform). 
        /// </summary> 
        public static Vector2 MulT(Mat22 A, Vector2 v)
        {
            Vector2 u;
            u = new Vector2(Vector2.Dot(v, A.col1), Vector2.Dot(v, A.col2));
            return u;
        }

        public static Mat22 MulT(Mat22 A, Mat22 B)
        {
            Vector2 c1 = new Vector2();
            Vector2 c2 = new Vector2();

            Vector2.Dot(ref A.col1, ref B.col1, out c1.X);
            Vector2.Dot(ref A.col2, ref B.col1, out c1.Y);
            Vector2.Dot(ref A.col1, ref B.col2, out c2.X);
            Vector2.Dot(ref A.col2, ref B.col2, out c2.Y);

            A.Set(c1, c2);
            return A;
        }

        public static Vector2 Mul(Transform T, Vector2 v)
        {
            return T.position + Math.Mul(T.R, v);
        }

        public static Vector2 MulT(Transform T, Vector2 v)
        {
            return Math.MulT(T.R, v - T.position);
        }

        // v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p)
        public static Transform MulT(Transform A, Transform B)
        {
            Transform C;
            C.R = Math.MulT(A.R, B.R);
            C.position = B.position - A.position;
            return C;
        }

        /// <summary> 
        /// Multiply a matrix times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another. 
        /// </summary> 
        public static void Mul(ref Mat22 A, ref Vector2 v, out Vector2 u)
        {
            u.X = A.col1.X * v.X + A.col2.X * v.Y;
            u.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
        }

    }

    /// A 2-by-2 matrix. Stored in column-major order.
    public struct Mat22
    {
	    /// The default constructor does nothing (for performance).
	    //b2Mat22() {}

	    /// Construct this matrix using columns.
	    public Mat22(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Construct this matrix using scalars.
	    public Mat22(float a11, float a12, float a21, float a22)
	    {
		    col1.X = a11; col1.Y = a21;
		    col2.X = a12; col2.Y = a22;
	    }

	    /// Construct this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
	    public Mat22(float angle)
	    {
		    // TODO_ERIN compute sin+cos together.
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
            col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

	    /// Initialize this matrix using columns.
	    public void Set(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Initialize this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
	    public void Set(float angle)
	    {
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
		    col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

        /// Initialize this matrix using an angle. This matrix becomes
        /// an orthonormal rotation matrix.
        public void Set(ref float angle)
        {
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
            col1.X = c; col2.X = -s;
            col1.Y = s; col2.Y = c;
        }

        /// Set this to the identity matrix.
	    public void SetIdentity()
	    {
		    col1.X = 1.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 1.0f;
	    }

	    /// Set this matrix to all zeros.
	    public void SetZero()
	    {
		    col1.X = 0.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 0.0f;
	    }

	    /// Extract the angle from this matrix (assumed to be
	    /// a rotation matrix).
	    public float GetAngle()
	    {
		    return (float)System.Math.Atan2(col1.Y, col1.X);
	    }

	    public Mat22 GetInverse()
	    {
		    float a = col1.X, b = col2.X, c = col1.Y, d = col2.Y;
		    Mat22 B = new Mat22();
		    float det = a * d - b * c;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = (float)(1.0f / det);
		    B.col1.X =  det * d;	B.col2.X = -det * b;
		    B.col1.Y = -det * c;	B.col2.Y =  det * a;
		    return B;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
	    public Vector2 Solve(ref Vector2 b)
	    {
		    float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
		    float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
		    Vector2 x;
		    x.X = det * (a22 * b.X - a12 * b.Y);
		    x.Y = det * (a11 * b.Y - a21 * b.X);
		    return x;
	    }

        /// Solve A * x = b, where b is a column vector. This is more efficient
        /// than computing the inverse in one-shot cases.
        public Vector2 Solve(Vector2 b)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
            Vector2 x;
            x.X = det * (a22 * b.X - a12 * b.Y);
            x.Y = det * (a11 * b.Y - a21 * b.X);
            return x;
        }

        public Vector2 Solve(ref float x, ref float y)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
            Vector2 x_ = Vector2.Zero;
            x_.X = det * (a22 * x - a12 * y);
            x_.Y = det * (a11 * y - a21 * x);
            return x_;
        }

	    public Vector2 col1, col2;

        /// Multiply a matrix times a vector. If a rotation matrix is provided,
        /// then this transforms the vector from one frame to another.
        public static Vector2 Mul(Mat22 A, Vector2 v)
        {
            Vector2 ret;
            ret.X = A.col1.X * v.X + A.col2.X * v.Y;
            ret.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
            return ret;
        }

        public static Mat22 operator + (Mat22 A, Mat22 B)
        {
            return new Mat22(A.col1 + B.col1, A.col2 + B.col2);
        }
    };

    /// A 3-by-3 matrix. Stored in column-major order.
    public struct Mat33
    {
	    /// The default constructor does nothing (for performance).
	    //b2Mat33() {}

	    /// Construct this matrix using columns.
	    public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
	    {
		    col1 = c1;
		    col2 = c2;
		    col3 = c3;
	    }

	    /// Set this matrix to all zeros.
	    public void SetZero()
	    {
		    col1 = Vector3.Zero;
		    col2 = Vector3.Zero;
		    col3 = Vector3.Zero;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
	    public Vector3 Solve33(Vector3 b)
        {
            float det = Vector3.Dot(col1, Vector3.Cross(col2, col3));
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }
            Vector3 x = Vector3.Zero;
            x.X = det * Vector3.Dot(b, Vector3.Cross(col2, col3));
            x.Y = det * Vector3.Dot(col1, Vector3.Cross(b, col3));
            x.Z = det * Vector3.Dot(col1, Vector3.Cross(col2, b));
            return x;
        }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases. Solve only the upper
	    /// 2-by-2 matrix equation.
        public Vector2 Solve22(Vector2 b)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }
            Vector2 x;
            x.X = det * (a22 * b.X - a12 * b.Y);
            x.Y = det * (a11 * b.Y - a21 * b.X);
            return x;
        }

	    public Vector3 col1, col2, col3;
    };

    /// A transform contains translation and rotation. It is used to represent
    /// the position and orientation of rigid frames.
    public struct Transform
    {
	    /// Initialize using a position vector and a rotation matrix.
	    public Transform(Vector2 position, Mat22 R) 
        {
            this.position = position;
            this.R = R;
        }

	    /// Set this to the identity transform.
	    public void SetIdentity()
	    {
		    position = Vector2.Zero;
		    R.SetIdentity();
	    }

	    /// Set this based on the position and angle.
	    public void Set(Vector2 p, float angle)
	    {
		    position = p;
		    R.Set(angle);
	    }

	    /// Calculate the angle that the rotation matrix represents.
	    public float GetAngle()
	    {
		    return (float)System.Math.Atan2(R.col1.Y, R.col1.X);
	    }

	    public Vector2 position;
	    public Mat22 R;

        public static Vector2 Mul(Transform T, Vector2 v)
        {
	        return T.position + Mat22.Mul(T.R, v);
        }
        
    };

    /// <summary>
    /// This describes the motion of a body/shape for TOI computation.
    /// Shapes are defined with respect to the body origin, which may
    /// no coincide with the center of mass. However, to support dynamics
    /// we must interpolate the center of mass position.
    /// </summary>
    public struct Sweep
    {
        /// <summary>Get the interpolated transform at a specific time.</summary>
	    /// <param name="beta">is a factor in [0,1], where 0 indicates alpha0.</param>
        public void GetTransform(out Transform xf, float beta)
        {
            xf = new Transform();
            xf.position = (1.0f - beta) * c0 + beta * c;
            float angle = (1.0f - beta) * a0 + beta * a;
            xf.R.Set(angle);

            // Shift to origin
            xf.position -= Math.Mul(xf.R, localCenter);
        }

        /// <summary>Advance the sweep forward, yielding a new initial state.</summary>
	    /// <param name="alpha">the new initial time.</param>
        public void Advance(float alpha)
        {
            System.Diagnostics.Debug.Assert(alpha0 < 1.0f);
            float beta = (alpha - alpha0) / (1.0f - alpha0);
            c0 = (1.0f - beta) * c0 + beta * c;
            a0 = (1.0f - beta) * a0 + beta * a;
            alpha0 = alpha;
        }

        /// <summary>
        /// Normalize the angles
        /// </summary>
        public void Normalize()
        {
            float d = (float)(Math.TwoPI * System.Math.Floor(a0 / Math.TwoPI));
            a0 -= d;
            a -= d;
        }

        public Vector2 localCenter;	// local center of mass position
        public Vector2 c0, c;		// center world positions
        public float a0, a;		    // world angles

        // Fraction of the current time step in the range [0,1]
        // c0 and a0 are the positions at alpha0.
        public float alpha0;
    };

#endif

    //
        // Optimized version of the port with special sections
        // for different plattforms
        //

#if OPTIMIZE

#if WINDOWS
    public static partial class Math
    {
        public static readonly ushort USHRT_MAX = 0xffff;
        public static readonly byte UCHAR_MAX = 0xff;
        public static readonly int RAND_LIMIT = 32767;

        public static readonly double PI = System.Math.PI;
        public static readonly double TwoPI = PI * 2.0;

        private static Random s_rnd = new Random();

        /// <summary>
        /// This function is used to ensure that a floating point number is
        /// not a NaN or infinity.
        /// </summary>
        public static bool IsValid(float x)
        {
            if (float.IsNaN(x)) return false;
            return float.NegativeInfinity < x && x < float.PositiveInfinity;
        }

        [StructLayout(LayoutKind.Explicit)]
        public struct Convert 
        { 
            [FieldOffset(0)]
            public float x;

            [FieldOffset(0)] 
            public int i;
        }

        /// <summary> 
        /// This is a approximate yet fast inverse square-root. 
        /// </summary> 
        public static float InvSqrt(float x)
        {
            Convert convert = new Convert();
            convert.x = x;
            float xhalf = 0.5f * x;
            convert.i = 0x5f3759df - (convert.i >> 1);
            x = convert.x;
            x = x * (1.5f - xhalf * x * x);
            return x;
        }

        /// <summary>
        /// Random number in range [-1,1]
        /// </summary>
        public static float Random()
        {
            float r = (float)(s_rnd.Next() & RAND_LIMIT);
            r /= RAND_LIMIT;
            r = 2.0f * r - 1.0f;
            return r;
        }

        public static Mat22 Abs(Mat22 A)
        {
            return new Mat22(A.col1.Abs(), A.col2.Abs());
        }

        public static void Swap<T>(ref T a, ref T b)
        {
            T tmp = a;
            a = b;
            b = tmp;
        }

        /// <summary> 
        /// Multiply a matrix times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another. 
        /// </summary> 
        public static Vector2 Mul(Mat22 A, Vector2 v)
        {
            Vector2 u;
            u.X = A.col1.X * v.X + A.col2.X * v.Y;
            u.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
            return u;
        }

        /// <summary> 
        /// Multiply a matrix transpose times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another (inverse transform). 
        /// </summary> 
        public static Vector2 MulT(Mat22 A, Vector2 v)
        {
            Vector2 u;
            Vector2.Dot(ref v, ref A.col1, out u.X);
            Vector2.Dot(ref v, ref A.col2, out u.Y);
            return u;
        }

        public static Mat22 MulT(Mat22 A, Mat22 B)
        {
            Vector2 c1;
            Vector2.Dot(ref A.col1, ref B.col1, out c1.X);
            Vector2.Dot(ref A.col2, ref B.col1, out c1.Y);
            
            Vector2 c2;
            Vector2.Dot(ref A.col1, ref B.col2, out c2.X);
            Vector2.Dot(ref A.col2, ref B.col2, out c2.Y);

            return new Mat22(c1, c2);
        }

        public static Vector2 Mul(Transform T, Vector2 v)
        {
            Vector2 t; t.X = T.R.col1.X * v.X + T.R.col2.X * v.Y; t.Y = T.R.col1.Y * v.X + T.R.col2.Y * v.Y;
            float x = T.position.X + t.X;
            float y = T.position.Y + t.Y;
            return new Vector2(x, y); 
        }

        public static Vector2 MulT(Transform T, Vector2 v)
        {
            Vector2 u;
            v = v - T.position;
            Vector2.Dot(ref v, ref T.R.col1, out u.X);
            Vector2.Dot(ref v, ref T.R.col2, out u.Y);
            return u;
        }

        // v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p)
        public static Transform MulT(Transform A, Transform B)
        {
            Transform C;
            C.R = Math.MulT(A.R, B.R);
            C.position = B.position - A.position;
            return C;
        }

        /// <summary> 
        /// Multiply a matrix times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another. 
        /// </summary> 
        public static void Mul(ref Mat22 A, ref Vector2 v, out Vector2 u)
        {
            u.X = A.col1.X * v.X + A.col2.X * v.Y;
            u.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
        }

    }

    /// A 2-by-2 matrix. Stored in column-major order.
    public struct Mat22
    {
	    /// The default constructor does nothing (for performance).
	    //b2Mat22() {}

	    /// Construct this matrix using columns.
	    public Mat22(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Construct this matrix using scalars.
	    public Mat22(float a11, float a12, float a21, float a22)
	    {
		    col1.X = a11; col1.Y = a21;
		    col2.X = a12; col2.Y = a22;
	    }

	    /// Construct this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
	    public Mat22(float angle)
	    {
		    // TODO_ERIN compute sin+cos together.
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
            col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

	    /// Initialize this matrix using columns.
	    public void Set(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Initialize this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
	    public void Set(float angle)
	    {
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
		    col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

        /// Initialize this matrix using an angle. This matrix becomes
        /// an orthonormal rotation matrix.
        public void Set(ref float angle)
        {
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
            col1.X = c; col2.X = -s;
            col1.Y = s; col2.Y = c;
        }

        /// Set this to the identity matrix.
	    public void SetIdentity()
	    {
		    col1.X = 1.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 1.0f;
	    }

	    /// Set this matrix to all zeros.
	    public void SetZero()
	    {
		    col1.X = 0.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 0.0f;
	    }

	    /// Extract the angle from this matrix (assumed to be
	    /// a rotation matrix).
	    public float GetAngle()
	    {
		    return (float)System.Math.Atan2(col1.Y, col1.X);
	    }

	    public Mat22 GetInverse()
	    {
		    float a = col1.X, b = col2.X, c = col1.Y, d = col2.Y;
		    Mat22 B = new Mat22();
		    float det = a * d - b * c;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = (float)(1.0f / det);
		    B.col1.X =  det * d;	B.col2.X = -det * b;
		    B.col1.Y = -det * c;	B.col2.Y =  det * a;
		    return B;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
	    public Vector2 Solve(ref Vector2 b)
	    {
		    float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
		    float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
		    Vector2 x = Vector2.Zero;
		    x.X = det * (a22 * b.X - a12 * b.Y);
		    x.Y = det * (a11 * b.Y - a21 * b.X);
		    return x;
	    }

        public Vector2 Solve(ref float x, ref float y)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
            Vector2 x_ = Vector2.Zero;
            x_.X = det * (a22 * x - a12 * y);
            x_.Y = det * (a11 * y - a21 * x);
            return x_;
        }

	    public Vector2 col1, col2;

        /// Multiply a matrix times a vector. If a rotation matrix is provided,
        /// then this transforms the vector from one frame to another.
        public static Vector2 Mul(Mat22 A, Vector2 v)
        {
            Vector2 ret = new Vector2();
            ret.X = A.col1.X * v.X + A.col2.X * v.Y;
            ret.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
            return ret;
        }

        public static Mat22 operator + (Mat22 A, Mat22 B)
        {
            return new Mat22(A.col1 + B.col1, A.col2 + B.col2);
        }
    };

    /// A 3-by-3 matrix. Stored in column-major order.
    public struct Mat33
    {
	    /// The default constructor does nothing (for performance).
	    //b2Mat33() {}

	    /// Construct this matrix using columns.
	    public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
	    {
		    col1 = c1;
		    col2 = c2;
		    col3 = c3;
	    }

	    /// Set this matrix to all zeros.
	    public void SetZero()
	    {
		    col1 = Vector3.Zero;
		    col2 = Vector3.Zero;
		    col3 = Vector3.Zero;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
	    public Vector3 Solve33(Vector3 b)
        {
            float det = Vector3.Dot(col1, Vector3.Cross(col2, col3));
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }
            Vector3 x = Vector3.Zero;
            x.X = det * Vector3.Dot(b, Vector3.Cross(col2, col3));
            x.Y = det * Vector3.Dot(col1, Vector3.Cross(b, col3));
            x.Z = det * Vector3.Dot(col1, Vector3.Cross(col2, b));
            return x;
        }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases. Solve only the upper
	    /// 2-by-2 matrix equation.
        public Vector2 Solve22(Vector2 b)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }
            Vector2 x;
            x.X = det * (a22 * b.X - a12 * b.Y);
            x.Y = det * (a11 * b.Y - a21 * b.X);
            return x;
        }

	    public Vector3 col1, col2, col3;
    };

    /// A transform contains translation and rotation. It is used to represent
    /// the position and orientation of rigid frames.
    public struct Transform
    {
        public static Transform Empty = new Transform();

	    /// Initialize using a position vector and a rotation matrix.
	    public Transform(Vector2 position, Mat22 R) 
        {
            this.position = position;
            this.R = R;
        }

	    /// Set this to the identity transform.
	    public void SetIdentity()
	    {
		    position = Vector2.Zero;
		    R.SetIdentity();
	    }

	    /// Set this based on the position and angle.
	    public void Set(Vector2 p, float angle)
	    {
		    position = p;
		    R.Set(angle);
	    }

	    /// Calculate the angle that the rotation matrix represents.
	    public float GetAngle()
	    {
		    return (float)System.Math.Atan2(R.col1.Y, R.col1.X);
	    }

	    public Vector2 position;
	    public Mat22 R;

        public static Vector2 Mul(Transform T, Vector2 v)
        {
	        return T.position + Mat22.Mul(T.R, v);
        }
        
    };

    /// <summary>
    /// This describes the motion of a body/shape for TOI computation.
    /// Shapes are defined with respect to the body origin, which may
    /// no coincide with the center of mass. However, to support dynamics
    /// we must interpolate the center of mass position.
    /// </summary>
    public struct Sweep
    {
        /// <summary>Get the interpolated transform at a specific time.</summary>
	    /// <param name="beta">is a factor in [0,1], where 0 indicates alpha0.</param>
        public void GetTransform(out Transform xf, float beta)
        {
            xf = Transform.Empty;
            xf.position = (1.0f - beta) * c0 + beta * c;
            float angle = (1.0f - beta) * a0 + beta * a;
            xf.R.Set(angle);

            // Shift to origin
            xf.position -= Math.Mul(xf.R, localCenter);
        }

        /// <summary>Advance the sweep forward, yielding a new initial state.</summary>
	    /// <param name="alpha">the new initial time.</param>
        public void Advance(float alpha)
        {
            System.Diagnostics.Debug.Assert(alpha0 < 1.0f);
            float beta = (alpha - alpha0) / (1.0f - alpha0);
            c0 = (1.0f - beta) * c0 + beta * c;
            a0 = (1.0f - beta) * a0 + beta * a;
            alpha0 = alpha;
        }

        /// <summary>
        /// Normalize the angles
        /// </summary>
        public void Normalize()
        {
            float d = (float)(Math.TwoPI * System.Math.Floor(a0 / Math.TwoPI));
            a0 -= d;
            a -= d;
        }

        public Vector2 localCenter;	// local center of mass position
        public Vector2 c0, c;		// center world positions
        public float a0, a;		    // world angles

        /// Fraction of the current time step in the range [0,1]
        /// c0 and a0 are the positions at alpha0.
        public float alpha0;
    };

#endif

#if XBOX || ZUNE || WINDOWS_PHONE
    public static partial class Math
    {
        public static readonly ushort USHRT_MAX = 0xffff;
        public static readonly byte UCHAR_MAX = 0xff;
        public static readonly int RAND_LIMIT = 32767;

        public static readonly double PI = System.Math.PI;
        public static readonly double TwoPI = PI * 2.0;

        private static Random s_rnd = new Random();

        #region Temporary variables
        static Vector2 u = Vector2.Zero;
        static Vector2 c1 = Vector2.Zero;
        static Vector2 c2 = Vector2.Zero;

        #endregion

        /// <summary>
        /// This function is used to ensure that a floating point number is
        /// not a NaN or infinity.
        /// </summary>
        public static bool IsValid(float x)
        {
            if (float.IsNaN(x)) return false;
            return float.NegativeInfinity < x && x < float.PositiveInfinity;
        }

        [StructLayout(LayoutKind.Explicit)]
        public struct Convert 
        { 
            [FieldOffset(0)]
            public float x;

            [FieldOffset(0)] 
            public int i;
        }

        /// <summary> 
        /// This is a approximate yet fast inverse square-root. 
        /// </summary> 
        public static float InvSqrt(float x)
        {
            Convert convert = new Convert();
            convert.x = x;
            float xhalf = 0.5f * x;
            convert.i = 0x5f3759df - (convert.i >> 1);
            x = convert.x;
            x = x * (1.5f - xhalf * x * x);
            return x;
        }

        /// <summary>
        /// Random number in range [-1,1]
        /// </summary>
        public static float Random()
        {
            float r = (float)(s_rnd.Next() & RAND_LIMIT);
            r /= RAND_LIMIT;
            r = 2.0f * r - 1.0f;
            return r;
        }

        public static Mat22 Abs(Mat22 A)
        {
            return new Mat22(A.col1.Abs(), A.col2.Abs());
        }

        public static void Swap<T>(ref T a, ref T b)
        {
            T tmp = a;
            a = b;
            b = tmp;
        }

        /// <summary> 
        /// Multiply a matrix times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another. 
        /// </summary> 
        public static Vector2 Mul(Mat22 A, Vector2 v)
        {
            u.X = A.col1.X * v.X + A.col2.X * v.Y;
            u.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
            return u;
        }

        /// <summary> 
        /// Multiply a matrix transpose times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another (inverse transform). 
        /// </summary> 
        public static Vector2 MulT(Mat22 A, Vector2 v)
        {
            Vector2.Dot(ref v, ref A.col1, out u.X);
            Vector2.Dot(ref v, ref A.col2, out u.Y);
            return u;
        }

        public static Mat22 MulT(Mat22 A, Mat22 B)
        {
            Vector2.Dot(ref A.col1, ref B.col1, out c1.X);
            Vector2.Dot(ref A.col2, ref B.col1, out c1.Y);
            
            Vector2.Dot(ref A.col1, ref B.col2, out c2.X);
            Vector2.Dot(ref A.col2, ref B.col2, out c2.Y);

            return new Mat22(c1, c2);
        }

        public static Vector2 Mul(Transform T, Vector2 v)
        {
            Vector2 t; t.X = T.R.col1.X * v.X + T.R.col2.X * v.Y; t.Y = T.R.col1.Y * v.X + T.R.col2.Y * v.Y;
            float x = T.position.X + t.X;
            float y = T.position.Y + t.Y;
            return new Vector2(x, y); 
        }

        public static Vector2 MulT(Transform T, Vector2 v)
        {
            v = v - T.position;
            Vector2.Dot(ref v, ref T.R.col1, out u.X);
            Vector2.Dot(ref v, ref T.R.col2, out u.Y);
            return u;
        }

        public static Transform MulT(Transform A, Transform B)
        {
            Transform C;
            C.R = Math.MulT(A.R, B.R);
            C.position = B.position - A.position;
            return C;
        }

        /// <summary> 
        /// Multiply a matrix times a vector. If a rotation matrix is provided, 
        /// then this transforms the vector from one frame to another. 
        /// </summary> 
        public static void Mul(ref Mat22 A, ref Vector2 v, out Vector2 u)
        {
            u = Vector2.Zero;
            u.X = A.col1.X * v.X + A.col2.X * v.Y;
            u.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
        }

    }

    /// A 2-by-2 matrix. Stored in column-major order.
    public struct Mat22
    {
        public Vector2 col1, col2;
        static Vector2 ret = Vector2.Zero;

	    /// The default constructor does nothing (for performance).
	    //b2Mat22() {}

	    /// Construct this matrix using columns.
	    public Mat22(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Construct this matrix using scalars.
	    public Mat22(float a11, float a12, float a21, float a22)
	    {
            col1 = new Vector2();
            col2 = new Vector2();
		    col1.X = a11; col1.Y = a21;
		    col2.X = a12; col2.Y = a22;
	    }

	    /// Construct this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
	    public Mat22(float angle)
	    {
		    // TODO_ERIN compute sin+cos together.
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
            col1 = new Vector2();
            col2 = new Vector2();
            col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

	    /// Initialize this matrix using columns.
	    public void Set(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Initialize this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
	    public void Set(float angle)
	    {
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
		    col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

        /// Initialize this matrix using an angle. This matrix becomes
        /// an orthonormal rotation matrix.
        public void Set(ref float angle)
        {
            float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle);
            col1.X = c; col2.X = -s;
            col1.Y = s; col2.Y = c;
        }

        /// Set this to the identity matrix.
	    public void SetIdentity()
	    {
		    col1.X = 1.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 1.0f;
	    }

	    /// Set this matrix to all zeros.
	    public void SetZero()
	    {
		    col1.X = 0.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 0.0f;
	    }

	    /// Extract the angle from this matrix (assumed to be
	    /// a rotation matrix).
	    public float GetAngle()
	    {
		    return (float)System.Math.Atan2(col1.Y, col1.X);
	    }

	    public Mat22 GetInverse()
	    {
		    float a = col1.X, b = col2.X, c = col1.Y, d = col2.Y;
		    Mat22 B = new Mat22();
		    float det = a * d - b * c;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = (float)(1.0f / det);
		    B.col1.X =  det * d;	B.col2.X = -det * b;
		    B.col1.Y = -det * c;	B.col2.Y =  det * a;
		    return B;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
	    public Vector2 Solve(ref Vector2 b)
	    {
		    float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
		    float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
		    Vector2 x = Vector2.Zero;
		    x.X = det * (a22 * b.X - a12 * b.Y);
		    x.Y = det * (a11 * b.Y - a21 * b.X);
		    return x;
	    }

        /// Solve A * x = b, where b is a column vector. This is more efficient
        /// than computing the inverse in one-shot cases.
        public Vector2 Solve(ref float x, ref float y)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            System.Diagnostics.Debug.Assert(det != 0.0f);
            det = 1.0f / det;
            //HACK: if (float.IsInfinity(det)) det = 1.0f;
            Vector2 x_ = Vector2.Zero;
            x_.X = det * (a22 * x - a12 * y);
            x_.Y = det * (a11 * y - a21 * x);
            return x_;
        }

        /// Multiply a matrix times a vector. If a rotation matrix is provided,
        /// then this transforms the vector from one frame to another.
        public static Vector2 Mul(Mat22 A, Vector2 v)
        {
            ret.X = A.col1.X * v.X + A.col2.X * v.Y;
            ret.Y = A.col1.Y * v.X + A.col2.Y * v.Y;
            return ret;
        }

        public static Mat22 operator + (Mat22 A, Mat22 B)
        {
            return new Mat22(A.col1 + B.col1, A.col2 + B.col2);
        }
    };

    /// A 3-by-3 matrix. Stored in column-major order.
    public struct Mat33
    {
	    /// The default constructor does nothing (for performance).
	    //b2Mat33() {}

	    /// Construct this matrix using columns.
	    public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
	    {
		    col1 = c1;
		    col2 = c2;
		    col3 = c3;
	    }

	    /// Set this matrix to all zeros.
	    public void SetZero()
	    {
		    col1 = Vector3.Zero;
		    col2 = Vector3.Zero;
		    col3 = Vector3.Zero;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
	    public Vector3 Solve33(Vector3 b)
        {
            float det = Vector3.Dot(col1, Vector3.Cross(col2, col3));
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }
            Vector3 x = Vector3.Zero;
            x.X = det * Vector3.Dot(b, Vector3.Cross(col2, col3));
            x.Y = det * Vector3.Dot(col1, Vector3.Cross(b, col3));
            x.Z = det * Vector3.Dot(col1, Vector3.Cross(col2, b));
            return x;
        }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases. Solve only the upper
	    /// 2-by-2 matrix equation.
        public Vector2 Solve22(Vector2 b)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }
            Vector2 x = Vector2.Zero;
            x.X = det * (a22 * b.X - a12 * b.Y);
            x.Y = det * (a11 * b.Y - a21 * b.X);
            return x;
        }

	    public Vector3 col1, col2, col3;
    };

    /// A transform contains translation and rotation. It is used to represent
    /// the position and orientation of rigid frames.
    public struct Transform
    {
        public static Transform Empty = new Transform();

        public Vector2 position;
        public Mat22 R;

	    /// Initialize using a position vector and a rotation matrix.
	    public Transform(Vector2 position, Mat22 R) 
        {
            this.position = position;
            this.R = R;
        }

	    /// Set this to the identity transform.
	    public void SetIdentity()
	    {
		    position = Vector2.Zero;
		    R.SetIdentity();
	    }

	    /// Set this based on the position and angle.
	    public void Set(Vector2 p, float angle)
	    {
		    position = p;
		    R.Set(angle);
	    }

	    /// Calculate the angle that the rotation matrix represents.
	    public float GetAngle()
	    {
		    return (float)System.Math.Atan2(R.col1.Y, R.col1.X);
	    }

        public static Vector2 Mul(Transform T, Vector2 v)
        {
	        return T.position + Mat22.Mul(T.R, v);
        }
        
    };

    /// <summary>
    /// This describes the motion of a body/shape for TOI computation.
    /// Shapes are defined with respect to the body origin, which may
    /// no coincide with the center of mass. However, to support dynamics
    /// we must interpolate the center of mass position.
    /// </summary>
    public struct Sweep
    {
        /// <summary>Get the interpolated transform at a specific time.</summary>
	    /// <param name="beta">is a factor in [0,1], where 0 indicates alpha0.</param>
        public void GetTransform(out Transform xf, float beta)
        {
            xf = Transform.Empty;
            xf.position = (1.0f - beta) * c0 + beta * c;
            float angle = (1.0f - beta) * a0 + beta * a;
            xf.R.Set(angle);

            // Shift to origin
            xf.position -= Math.Mul(xf.R, localCenter);
        }

        /// <summary>Advance the sweep forward, yielding a new initial state.</summary>
	    /// <param name="alpha">the new initial time.</param>
        public void Advance(float alpha)
        {
            System.Diagnostics.Debug.Assert(alpha0 < 1.0f);
            float beta = (alpha - alpha0) / (1.0f - alpha0);
            c0 = (1.0f - beta) * c0 + beta * c;
            a0 = (1.0f - beta) * a0 + beta * a;
            alpha0 = alpha;
        }

        /// <summary>
        /// Normalize the angles
        /// </summary>
        public void Normalize()
        {
            float d = (float)(Math.TwoPI * System.Math.Floor(a0 / Math.TwoPI));
            a0 -= d;
            a -= d;
        }

        public Vector2 localCenter;	// local center of mass position
        public Vector2 c0, c;		// center world positions
        public float a0, a;		    // world angles

        /// Fraction of the current time step in the range [0,1]
        /// c0 and a0 are the positions at alpha0.
        public float alpha0;
    };

#endif

#endif

}
